#include <stdio.h>
#include <stdlib.h>
#include "fdecl.h"
#include <math.h>

hybroid *make_hybroid (FILE *infile)
{
  double pi = 3.14159265358979323;
  hybroid *h;
  vector initial_axis;

  char dummy[100];

  h = (hybroid *) calloc(1, sizeof(hybroid));


  fscanf(infile, "%[^\n]", dummy);
  fscanf(infile, "%d %d", &h->usize, &h->vsize);
  printf("Making hybroid size (usize, vsize) = (%d, %d)\n", 
	 h->usize, h->vsize);
  h->ustep = pi / (h->usize + 1); h->vstep = 2.0 * pi / h->vsize;
  h->ubegin = (-pi / 2.0) + h->ustep; h->uend = (pi / 2.0) - h->ustep;
  h->vbegin = -pi; h->vend = pi - h->vstep;
  
  allocate_hybroid(h);

  fscanf(infile, "\n\n");
  fscanf(infile, "%[^\n]", dummy);
  fscanf(infile, "%lf %lf", &h->w0, &h->w1);
  printf("Deformation parameters: w0 = %f, w1 = %f\n", h->w0, h->w1);
  fscanf(infile, "\n\n");
  fscanf(infile, "%[^\n]", dummy);
  fscanf(infile, "%lf %lf %lf %lf %lf %lf", 
	 &h->sc, &h->a1, &h->a2, &h->a3, &h->e1, &h->e2);
  printf("Superquadric parameters: sc = %f, a1 = %f, a2 = %f, a3 = %f, e1 = %f, e2 = %f\n", 
	 h->sc, h->a1, h->a2, h->a3, h->e1, h->e2);
  fscanf(infile, "\n\n");
  fscanf(infile, "%[^\n]", dummy);
  fscanf(infile, "%lf %lf %lf %lf %lf", 
	 &h->tapx, &h->tapy, &h->bend1, &h->bend2, &h->bend3);
  printf("Tapering and Bending parameters: tapx = %f, tapy = %f, bend1 = %f, bend2 = %f, bend3 = %f\n", 
	 h->tapx, h->tapy, h->bend1, h->bend2, h->bend3);

  fscanf(infile, "\n\n");
  fscanf(infile, "%[^\n]", dummy);
  fscanf(infile, "%lf", 
	 &h->dt);
  printf("Dynamics parameters: dt = %f\n", 
	 h->dt);

  /* initialize hybroid translation and rotation */
  vzeroize(h->translation);
  vzeroize(initial_axis);
  unit_quaternion(0.0, initial_axis, h->orientation);
  unit_quaternion_to_rotation_matrix(h->orientation, h->rotation);
  /* zeroize tapering and bending derivatives */
  h->dtapx = 0.0; h->dtapy = 0.0; h->dbend1 = 0.0; 
  h->dbend2 = 0.0; h->dbend3 = 0.0;
 
  fclose(infile);
  return h;
}


//Mingchen: print node positions of the hybroid
void printh(hybroid *h){
	printf("frint h\n");
	node *p, *nds = h->nodes;
	p = nds;
	for (int i = 0; i < h->numnodes; i++){
		printf("%f %f %f\n", p->posit.x, p->posit.y, p->posit.z);
		p++;
	}

}


void allocate_hybroid(hybroid *h)
{
 int i,j;

  h->numnodes = h->usize * h->vsize + 2;
/* to be used with rectangular elements */
/*
  h->numelems = (h->usize - 1) * h->vsize + 2 * h->vsize;
*/
  h->numelems = 2 * (h->usize - 1) * h->vsize + 2 * h->vsize;
  h->nodes = (node *) calloc((unsigned)h->numnodes, (unsigned)sizeof(node)); 
  h->elems = (elem *) calloc((unsigned)h->numelems, (unsigned)sizeof(elem));
  printf("Element number:elems = %d\n", h->numelems);

  /* allocate pointer arrays for poles */
  
  h->nodes[h->numnodes - 2].many_neighbors = 
    (struct node **) calloc((unsigned)h->vsize, (unsigned)sizeof(struct node*));
  h->nodes[h->numnodes - 1].many_neighbors = 
    (struct node **) calloc((unsigned)h->vsize, (unsigned)sizeof(struct node*));

/* addition to include pointers to elements */
/* no need to zeroize max_elem_num for polar nodes */

  h->nodes[h->numnodes - 2].elements =
    (struct elem **) calloc((unsigned) h->vsize, (unsigned)sizeof(struct elem*));
    h->nodes[h->numnodes - 2].corr_number =
    (int *) calloc((unsigned)h->vsize, (unsigned)sizeof(int));
    h->nodes[h->numnodes - 2].max_elem_num = 0;

  h->nodes[h->numnodes - 1].elements =
    (struct elem **) calloc((unsigned) h->vsize, (unsigned)sizeof(struct elem*));
    h->nodes[h->numnodes - 1].corr_number =
    (int *) calloc((unsigned)h->vsize, (unsigned)sizeof(int));
    h->nodes[h->numnodes - 1].max_elem_num = 0;


  /* a node can correspond 6 or 5 different elements */
  for(i = 0; i < h->numnodes-2; i++) {
    h->nodes[i].elements =
      (struct elem **) calloc((unsigned) 6, (unsigned)sizeof(struct elem*));
    h->nodes[i].max_elem_num = 0;

    h->nodes[i].corr_number =
      (int *) calloc((unsigned) 6, (unsigned)sizeof(int));
    for(j=0; j < 6; j++)
      h->nodes[i].corr_number[j] = -1;
  }

}


void init_rect_uv_coords(hybroid *h)
{
  int i, j;
  double u, v, pi = 3.14159265358979323; /* fix me */
  node *p, *nds = h->nodes;

  int id = 0;	// Zhennan: node id

  p = nds;
  for (u = h->ubegin, i = 0; i < h->usize ; u += h->ustep, i++)
    for (v = h->vbegin, j = 0; j < h->vsize; v += h->vstep, j++) {
      p->uu = u; p->vv = v;
	  p->node_id = id++;
      p++;
    }
  p->uu = -pi/2.0; p->vv = 0.0;	/* south pole */
  p->node_id = id++;
  p++;
  p->uu = pi/2.0; p->vv = 0.0;	/* north pole */
  p->node_id = id++;
  p++;
}


void assemble_hybroid_rectangular_mesh(hybroid *h)
{
  int up, u, un, vp, v, vn, celm = 0;
  int umax = h->usize - 1, vmax = h->vsize - 1, vs = h->vsize;
  int south_pole = h->numnodes - 2, north_pole = h->numnodes - 1;
  node *nds = h->nodes;
  elem *els = h->elems;

  init_rect_uv_coords(h);
 
  /* make torroidal mesh */
  for (up = umax, u = 0, un = 1; u <= umax; 
       up = u, u++, un = (u == umax) ? 0 : u+1)
    for (vp = vmax, v = 0, vn = 1; v <= vmax;
	 vp = v, v++, vn = (v == vmax) ? 0 : v+1) {
      nds[u * vs + v].neighbors[0] = &nds[up * vs + v]; /* south */
      nds[u * vs + v].neighbors[1] = &nds[u * vs + vp];	/* west */
      nds[u * vs + v].neighbors[2] = &nds[u * vs + vn];	/* east */
      nds[u * vs + v].neighbors[3] = &nds[un * vs + v];	/* north */
      nds[u * vs + v].num_neighbors = 4;



      /* make all the 4 node elements */
      if (u != umax) {
#if 0
        els[celm].vertices[0] = &nds[u * vs + v]; /* 1st vertex */
        els[celm].vertices[1] = &nds[un * vs + v]; /* 2nd vertex */
        els[celm].vertices[2] = &nds[un * vs + vn]; /* 3rd vertex */
        els[celm].vertices[3] = &nds[u * vs + vn]; /* 4th vertex */
        els[celm].nomnodes = 4;                    /* number of nodes */
        els[celm].elemtype = 0;                   /* element type */
        celm++;
#endif

        els[celm].vertices[0] = &nds[u * vs + v];
        els[celm].vertices[1] = &nds[un * vs + v];
        els[celm].vertices[2] = &nds[u * vs + vn];  
        els[celm].nomnodes = 3;                    /* number of nodes */
        els[celm].elemtype = 2;                   /* element type */

	/* initialization of pointers from nodes to elements */
        nds[u * vs + v].elements[nds[u * vs + v].max_elem_num] = &els[celm];
        nds[u * vs + v].corr_number[nds[u * vs + v].max_elem_num] = 0;
        nds[u * vs + v].max_elem_num++;

        nds[un * vs + v].elements[nds[un * vs + v].max_elem_num] = &els[celm];
        nds[un * vs + v].corr_number[nds[un * vs + v].max_elem_num] = 1;
        nds[un * vs + v].max_elem_num++;

        nds[u * vs + vn].elements[nds[u * vs + vn].max_elem_num] = &els[celm];
        nds[u * vs + vn].corr_number[nds[u * vs + vn].max_elem_num] = 2;
        nds[u * vs + vn].max_elem_num++;

        celm++;

        els[celm].vertices[0] = &nds[un * vs + v];
        els[celm].vertices[1] = &nds[un * vs + vn];
        els[celm].vertices[2] = &nds[u * vs + vn];  
        els[celm].nomnodes = 3;                    /* number of nodes */
        els[celm].elemtype = 3;                   /* element type */

	/* initialization of pointers from nodes to elements */
       nds[un * vs + v].elements[nds[un * vs + v].max_elem_num] = &els[celm];
       nds[un * vs + v].corr_number[nds[un * vs + v].max_elem_num] = 0;
       nds[un * vs + v].max_elem_num++;

       nds[un * vs + vn].elements[nds[un * vs + vn].max_elem_num] = &els[celm];
       nds[un * vs + vn].corr_number[nds[un * vs + vn].max_elem_num] = 1;
       nds[un * vs + vn].max_elem_num++;

       nds[u * vs + vn].elements[nds[u * vs + vn].max_elem_num] = &els[celm];
       nds[u * vs + vn].corr_number[nds[u * vs + vn].max_elem_num] = 2;
       nds[u * vs + vn].max_elem_num++;

	celm++;


      }
    }
  
  
  /* undo torus; make spherical mesh with polar caps */
  for (v = 0, vn = 1; v <= vmax; v++, vn = (v == vmax) ? 0 : v+1) {
    nds[0 * vs + v].neighbors[0] = &nds[south_pole];	/* south */
    nds[south_pole].many_neighbors[v] = &nds[0 * vs + v];
    nds[umax * vs + v].neighbors[3] = &nds[north_pole];	/* north */
    nds[north_pole].many_neighbors[v] = &nds[umax * vs + v];


    /* make all the 3 node elements */
    els[celm].vertices[0] = &nds[south_pole]; /* first vertex */
    els[celm].vertices[1] = &nds[0 * vs + v]; /* second vertex */
    els[celm].vertices[2] = &nds[0 * vs + vn]; /* third vertex */
    els[celm].nomnodes = 3;              /* number of nodes */
    els[celm].elemtype = 1;             /* element type */

    /* initialization of pointers from nodes to elements */
    nds[south_pole].elements[nds[south_pole].max_elem_num] = &els[celm];
    nds[south_pole].corr_number[nds[south_pole].max_elem_num] = 0;
    nds[south_pole].max_elem_num++;
    
    nds[0 * vs + v].elements[nds[0 * vs + v].max_elem_num] = &els[celm];
    nds[0 * vs + v].corr_number[nds[0 * vs + v].max_elem_num] = 1;
    nds[0 * vs + v].max_elem_num++;
    
    nds[0 * vs + vn].elements[nds[0 * vs + vn].max_elem_num] = &els[celm];
    nds[0 * vs + vn].corr_number[nds[0 * vs + vn].max_elem_num] = 2;
    nds[0 * vs + vn].max_elem_num++;

    celm++;

    els[celm].vertices[0] = &nds[umax * vs + v]; /* first vertex */
    els[celm].vertices[1] = &nds[north_pole];    /* second vertex */
    els[celm].vertices[2] = &nds[umax * vs + vn]; /* third vertex */
    els[celm].nomnodes = 3;                /* number of nodes */
    els[celm].elemtype = 2;                /* element type */

    /* initialization of pointers from nodes to elements */
    nds[umax * vs + v].elements[nds[umax * vs + v].max_elem_num] = &els[celm];
    nds[umax * vs + v].corr_number[nds[umax * vs + v].max_elem_num] = 0;
    nds[umax * vs + v].max_elem_num++;

    nds[north_pole].elements[nds[north_pole].max_elem_num] = &els[celm];
    nds[north_pole].corr_number[nds[north_pole].max_elem_num] = 1;
    nds[north_pole].max_elem_num++;
    
   nds[umax * vs + vn].elements[nds[umax * vs + vn].max_elem_num] = &els[celm];
   nds[umax * vs + vn].corr_number[nds[umax * vs + vn].max_elem_num] = 2;
   nds[umax * vs + vn].max_elem_num++;


    celm++;
  }
  nds[south_pole].num_neighbors = nds[north_pole].num_neighbors = vs;
  printf("Made number of elemnts:celm = %d\n", celm);
    /* create the array of elements */
  
}
    







  
